Multivariate KF Examples

Example 1 – vehicle location estimation

We design a six-dimensional Kalman Filter without control input. That is, we estimate a vehicle’s location on the XY plane. The vehicle has an on-board location sensor that reports X and Y coordinates of the system. We assume constant acceleration dynamics.

Kalman Filter equations

There is no control input so: \(\textbf{u} = 0\)

The state extrapolation equation: \[\hat{\textbf{x}}_{n+1, n} = \textbf{F} \hat{\textbf{x}}_{n, n}\]

The system state \(x_{n}\) is defined by: \[ \textbf{x}_{n} = \begin{bmatrix} x_{n} \\ \dot{x}_{n} \\ \ddot{x}_{n} \\ y_{n} \\ \dot{y}_{n} \\ \ddot{y}_{n} \end{bmatrix} \]

So the extrapolated vehicle state for time n + 1 can be described by:

\(\begin{array}{ccc} \begin{bmatrix} \hat{x}_{n+1,n} \\ \hat{\dot{x}}_{n+1,n} \\ \hat{\ddot{x}}_{n+1,n} \\ \hat{y}_{n+1,n} \\ \hat{\dot{y}}_{n+1,n} \\ \hat{\ddot{y}}_{n+1,n} \end{bmatrix} = & \begin{bmatrix} 1 & \Delta t & 0.5 \Delta t^2 & 0 & 0 & 0\\ 0 & 1 & \Delta t & 0 & 0 & 0\\ 0 & 0 & 1 & 0 & 0 & 0\\ 0 & 0 & 0 & 1 & \Delta t & 0.5 \Delta t^2\\ 0 & 0 & 0 & 0 & 1 & \Delta t \\ 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix} & \begin{bmatrix} \hat{x}_{n,n} \\ \hat{\dot{x}}_{n,n} \\ \hat{\ddot{x}}_{n,n} \\ \hat{y}_{n,n} \\ \hat{\dot{y}}_{n,n} \\ \hat{\ddot{y}}_{n,n} \end{bmatrix} \end{array}\)

The covariance extrapolation equation:

The estimate covariance matrix is, assuming that the estimation error in \(X\) and \(Y\) axes are not correlated: \[ P = \begin{bmatrix} p_{x} & p_{x\dot{x}} & p_{x\ddot{x}} & 0 & 0 & 0 \\ p_{\dot{x}x} & p_{\dot{x}} & p_{\dot{x}\ddot{x}} & 0 & 0 & 0 \\ p_{\ddot{x}x} & p_{\ddot{x}\dot{x}} & p_{\ddot{x}} & 0 & 0 & 0\\ 0 & 0 & 0 & p_{y} & p_{y\dot{y}} & p_{y\ddot{y}} \\ 0 & 0 & 0 & p_{\dot{y}y} & p_{\dot{y}} & p_{\dot{y}\ddot{y}} \\ 0 & 0 & 0 & p_{\ddot{y}y} & p_{\ddot{y}\dot{y}} & p_{\ddot{y}} \\ \end{bmatrix} \]

The process noise matrix, assuming a discrete noise model: \[\begin{align*} Q = & \begin{bmatrix} \sigma^2_{x} & \sigma^2_{x\dot{x}} & \sigma^2_{x\ddot{x}} & 0 & 0 & 0 \\ \sigma^2_{\dot{x}x} & \sigma^2_{\dot{x}} & \sigma^2_{\dot{x}\ddot{x}} & 0 & 0 & 0 \\ \sigma^2_{\ddot{x}x} & \sigma^2_{\ddot{x}\dot{x}} & \sigma^2_{\ddot{x}} & 0 & 0 & 0\\ 0 & 0 & 0 & \sigma^2_{y} & \sigma^2_{y\dot{y}} & \sigma^2_{y\ddot{y}} \\ 0 & 0 & 0 & \sigma^2_{\dot{y}y} & \sigma^2_{\dot{y}} & \sigma^2_{\dot{y}\ddot{y}} \\ 0 & 0 & 0 & \sigma^2_{\ddot{y}y} & \sigma^2_{\ddot{y}\dot{y}} & \sigma^2_{\ddot{y}} \\ \end{bmatrix} \\ &= \sigma^2_{a} \begin{bmatrix} \frac{\Delta t^4}{4} & \frac{\Delta t^3}{2} & \frac{\Delta t^2}{2} & 0 & 0 & 0 \\ \frac{\Delta t^3}{2} & \Delta t^2 & \Delta t & 0 & 0 & 0 \\ \frac{\Delta t^2}{2} & \Delta t & 1 & 0 & 0 & 0\\ 0 & 0 & 0 & \frac{\Delta t^4}{4} & \frac{\Delta t^3}{2} & \frac{\Delta t^2}{2} \\ 0 & 0 & 0 & \frac{\Delta t^3}{2} & \Delta t^2 & \Delta t \\ 0 & 0 & 0 & \frac{\Delta t^2}{2} & \Delta t & 1 \\ \end{bmatrix} \end{align*}\]

The measurement equation:

The measurement provides us only \(X\) and \(Y\) coordinates of the vehicle: \[ z_{n} = \begin{bmatrix} x_{n, measured} \\ y_{n, measured} \end{bmatrix} \]

The dimension of \(z_{n}\) is \(2×1\) and the dimension of \(x_{n}\) is \(6×1\). Therefore the dimension of the observation matrix \(H\) shall be \(2 × 6\). \[ \textbf{H} = \begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 \end{bmatrix} \]

The measurement uncertainty, assuming that the \(x\) and \(y\) measurements are uncorrelated: \[ \textbf{R}_{n} = \begin{bmatrix} \sigma^2_{x_{m}} & 0 \\ 0 & \sigma^2_{y_{m}} \end{bmatrix} \]

The subscript \(m\) is for measurement uncertainty, but for this example we assume a constant measurement uncertainty: \[\textbf{R}_{1} = ... = \textbf{R}_{n}=\textbf{R}\]

So we have defined all the necessary blocks for the Kalman Gain, The state update equation and The covariance update equation.

Numerical example

Let us assume a vehicle moving in a straight line in the X direction with a constant velocity. After traveling 400 meters the vehicle turns right, with a turning radius of 300 meters. During the turning maneuver, the vehicle experiences acceleration due to the circular motion (angular acceleration).

# The measurements period (s)
deltaT = 1

# The random acceleration standard deviation (m/s^2)
sigma_a = 0.2

# The measurement error standard deviation (m)
sigma_xm = 3
sigma_ym = 3

# The state transition matrix F would be
vec1 <- c(1, 1, 0.5, 0, 0, 0)
vec2 <- c(0, 1, 1, 0, 0, 0)
vec3 <- c(0, 0, 1, 0, 0, 0)
vec4 <- c(0, 0, 0, 1, 1, 0.5)
vec5 <- c(0, 0, 0, 0, 1, 1)
vec6 <- c(0, 0, 0, 0, 0, 1)
transMat <- rbind(vec1, vec2, vec3, vec4, vec5, vec6); transMat
##      [,1] [,2] [,3] [,4] [,5] [,6]
## vec1    1    1  0.5    0    0  0.0
## vec2    0    1  1.0    0    0  0.0
## vec3    0    0  1.0    0    0  0.0
## vec4    0    0  0.0    1    1  0.5
## vec5    0    0  0.0    0    1  1.0
## vec6    0    0  0.0    0    0  1.0
# The process noise matrix Q would be
vec1 <- c(1/4, 1/2, 1/2, 0, 0, 0)
vec2 <- c(1/2, 1, 1, 0, 0, 0)
vec3 <- c(1/2, 1, 1, 0, 0, 0)
vec4 <- c(0, 0, 0, 1/4, 1/2, 1/2)
vec5 <- c(0, 0, 0, 1/2, 1, 1)
vec6 <- c(0, 0, 0, 1/2, 1, 1)
processNoiseMat <- rbind(vec1, vec2, vec3, vec4, vec5, vec6) * 0.2^2
processNoiseMat
##      [,1] [,2] [,3] [,4] [,5] [,6]
## vec1 0.01 0.02 0.02 0.00 0.00 0.00
## vec2 0.02 0.04 0.04 0.00 0.00 0.00
## vec3 0.02 0.04 0.04 0.00 0.00 0.00
## vec4 0.00 0.00 0.00 0.01 0.02 0.02
## vec5 0.00 0.00 0.00 0.02 0.04 0.04
## vec6 0.00 0.00 0.00 0.02 0.04 0.04
# The measurement covariance R would be
R <- rbind(c(9, 0), c(0, 9)); R
##      [,1] [,2]
## [1,]    9    0
## [2,]    0    9
# Observation matrix
H <- rbind(c(1, 0, 0, 0, 0, 0), c(0, 0, 0, 1, 0, 0)); H
##      [,1] [,2] [,3] [,4] [,5] [,6]
## [1,]    1    0    0    0    0    0
## [2,]    0    0    0    1    0    0
# The set of 35 noisy measurements
x <- c(301.5, 298.23, 297.83, 300.42, 301.94, 299.5, 305.98, 301.25, 299.73, 299.2, 298.62, 301.84, 299.6, 295.3, 299.3, 301.95, 296.3, 295.11, 295.12, 289.9, 283.51, 276.42, 264.22, 250.25, 236.66, 217.47, 199.75, 179.7, 160, 140.92, 113.53, 93.68, 69.71, 45.93, 20.87)

y <- c(-401.46, -375.44, -346.15, -320.2, -300.08, -274.12, -253.45, -226.4, -200.65, -171.62, -152.11, -125.19, -93.4, -74.79, -49.12, -28.73, 2.99, 25.65, 49.86, 72.87, 96.34, 120.4, 144.69, 168.06, 184.99, 205.11, 221.82, 238.3, 253.02, 267.19, 270.71, 285.86, 288.48, 292.9, 298.77)

zn <- cbind(x,y)

Iteration Zero

# Initialization
x00 <- c(0, 0, 0, 0, 0, 0) # as we do not know the vehicle location

# Since is a guess, we set a very high estimate uncertainty 
  # The high estimate uncertainty results in a high Kalman Gain by giving a high weight to the measurement.
P00 <- diag(c(500, 500, 500, 500, 500, 500))

Prediction

x10 <- transMat %*% x00; x10
##      [,1]
## vec1    0
## vec2    0
## vec3    0
## vec4    0
## vec5    0
## vec6    0
P10 <- transMat %*% P00 %*% t(transMat) + processNoiseMat; P10
##         vec1    vec2   vec3    vec4    vec5   vec6
## vec1 1125.01  750.02 250.02    0.00    0.00   0.00
## vec2  750.02 1000.04 500.04    0.00    0.00   0.00
## vec3  250.02  500.04 500.04    0.00    0.00   0.00
## vec4    0.00    0.00   0.00 1125.01  750.02 250.02
## vec5    0.00    0.00   0.00  750.02 1000.04 500.04
## vec6    0.00    0.00   0.00  250.02  500.04 500.04

First Iteration

# Step 1 - Measure
  # The measurement values:
z1 <- c(x[1], y[1]); z1
## [1]  301.50 -401.46
# Step 2 - Update
  # The Kalman Gain calculation:
k1 <- P10 %*% t(H) %*% solve(H %*% P10 %*% t(H) + R); k1
##           [,1]      [,2]
## vec1 0.9920636 0.0000000
## vec2 0.6613875 0.0000000
## vec3 0.2204742 0.0000000
## vec4 0.0000000 0.9920636
## vec5 0.0000000 0.6613875
## vec6 0.0000000 0.2204742

Therefore, we can see that the Kalman Gain for a position is 0.9921, which means that the weight of the first measurement is significantly higher than the weight of the estimation; that is the weight of the estimation is negligible.

# Estimate the current state:
x11 <- x10 + k1 %*% (z1 - H %*% x10); x11
##            [,1]
## vec1  299.10716
## vec2  199.40832
## vec3   66.47299
## vec4 -398.27384
## vec5 -265.52061
## vec6  -88.51159
# Update the current estimate uncertainty
P11 <- (diag(6) - k1 %*% H) %*% P10 %*% t(diag(6) - k1 %*% H) + k1 %*% R %*% t(k1); P11
##          vec1       vec2       vec3     vec4       vec5       vec6
## vec1 8.928572   5.952487   1.984268 0.000000   0.000000   0.000000
## vec2 5.952487 503.986173 334.679906 0.000000   0.000000   0.000000
## vec3 1.984268 334.679906 444.917029 0.000000   0.000000   0.000000
## vec4 0.000000   0.000000   0.000000 8.928572   5.952487   1.984268
## vec5 0.000000   0.000000   0.000000 5.952487 503.986173 334.679906
## vec6 0.000000   0.000000   0.000000 1.984268 334.679906 444.917029
# Step 3 - Predict
x21 <- transMat %*% x11; x21
##            [,1]
## vec1  531.75198
## vec2  265.88131
## vec3   66.47299
## vec4 -708.05025
## vec5 -354.03220
## vec6  -88.51159
P21 <- transMat %*% P11 %*% t(transMat) + processNoiseMat; P21
##           vec1      vec2     vec3      vec4      vec5     vec6
## vec1  972.7232 1236.4213 559.1427    0.0000    0.0000   0.0000
## vec2 1236.4213 1618.3030 779.6369    0.0000    0.0000   0.0000
## vec3  559.1427  779.6369 444.9570    0.0000    0.0000   0.0000
## vec4    0.0000    0.0000   0.0000  972.7232 1236.4213 559.1427
## vec5    0.0000    0.0000   0.0000 1236.4213 1618.3030 779.6369
## vec6    0.0000    0.0000   0.0000  559.1427  779.6369 444.9570
 # the prediction uncertainty is still very high

Generalization

multiKF <- function(transMat, processNoiseMat, H, R, iniX, iniP, zn){
  
  # Initialization
  x <- list(iniX)
  p <- list(iniP)
  k <- list(0)
  
  # Prediction
  x <- append(x, list(transMat %*% x[[1]]))
  p <- append(p, list(transMat %*% p[[1]] %*% t(transMat) + processNoiseMat))
  
  for (i in 2:dim(zn)[1]){
    # Measure
    z <- c(zn[i-1, 1], zn[i-1, 2])
    # print(paste0('z ', z))
    
    # Update
    k <- append(k, list(p[[length(p)]] %*% t(H) %*% solve(H %*% p[[length(p)]] %*% t(H) + R)))
    # print(paste0('k', k))
    
    # Estimate the current state
    x <- append(x, list(x[[length(x)]] + k[[i]] %*% (z - H %*% x[[length(x)]])))
    # print(paste0('x', x))
    
    # Update the current estimate uncertainty
    p <- append(p, list((diag(dim(k[[i]])[1]) - k[[i]] %*% H) %*% p[[length(p)]] %*% t(diag(dim(k[[i]])[1]) - k[[i]] %*% H) + k[[i]] %*% R %*% t(k[[i]])))
    # print(paste0('p', p))
    
    # Predict
    x <- append(x, list(transMat %*% x[[length(x)]]))
    # print(paste0('x', x))
    
    p <- append(p, list(transMat %*% p[[length(p)]] %*% t(transMat) + processNoiseMat))
    # print(paste0('p', p))
  }
  return(list(x, p, k))
}

result <- multiKF(transMat, processNoiseMat, H, R, x00, P00, zn)
x <- result[[1]]
p <- result[[2]]
k <- result[[3]]

Visualization

Vehicle Position:

library(ggplot2)
library(dplyr)
## 
## Attaching package: 'dplyr'
## The following objects are masked from 'package:stats':
## 
##     filter, lag
## The following objects are masked from 'package:base':
## 
##     intersect, setdiff, setequal, union
library(plotly)
## 
## Attaching package: 'plotly'
## The following object is masked from 'package:ggplot2':
## 
##     last_plot
## The following object is masked from 'package:stats':
## 
##     filter
## The following object is masked from 'package:graphics':
## 
##     layout
# Take the estimates in the x axis
estimatesX <- c(300)
for (i in seq(3, 70, by = 2)){
    estimatesX = append(estimatesX, list(x[[i]][1]))
}

# Take the estimates in the y axis
estimatesY <- c(-400)
for (i in seq(3, 70, by = 2)){
    estimatesY = append(estimatesY, list(x[[i]][4]))
}
# Dataframa with all the data
data <- data.frame(
  estimX = unlist(estimatesX), # only the estimates
  estimY = unlist(estimatesY),
  measurements = zn
)

# True value?
# Plot to compare the true value, measured values and estimates
plotPos <- ggplot(data) +
  geom_path(aes(x = estimX, y = estimY, color = "Estimates")) +
  geom_point(aes(x = estimX, y = estimY, color = "Estimates", shape = "Estimates")) +
  
 geom_path(aes(x = measurements.x, y = measurements.y, color = "Measurements")) +
  geom_point(aes(x = measurements.x, y = measurements.y, color = "Measurements", shape = "Measurements")) +
  
  labs(title = "Vehicle Position",
       x = "X(m)",
       y = "Y(m)",
       color = "") +
  
  scale_color_manual(values = c("Estimates" = "red", "Measurements" = "blue", "True values" = "green"), name = "") +
  scale_shape_manual(values = c("Estimates" = 19, "Measurements" = 15, "True values" = 18), name = "")

ggplotly(plotPos) %>% config(displayModeBar = TRUE)
library(SIBER)

plot(x[[4]][1], x[[4]][4])

sigma2 <- rbind(c(p[[4]][4,4], 0),
                 c(0, p[[4]][4,4]))

mu <- c(x[[4]][1], x[[4]][4])

addEllipse(mu, sigma2, p.interval = 0.95, fill = "yellow", alpha = 0.2)
## Warning in plot.xy(xy.coords(x, y), type = type, ...): "fill" is not a
## graphical parameter
## Warning in plot.xy(xy.coords(x, y), type = type, ...): "alpha" is not a
## graphical parameter

##            [,1]      [,2]
##   [1,] 608.0936 -708.0502
##   [2,] 607.9399 -703.2084
##   [3,] 607.4794 -698.3860
##   [4,] 606.7140 -693.6025
##   [5,] 605.6467 -688.8772
##   [6,] 604.2819 -684.2291
##   [7,] 602.6250 -679.6769
##   [8,] 600.6828 -675.2390
##   [9,] 598.4630 -670.9332
##  [10,] 595.9746 -666.7769
##  [11,] 593.2276 -662.7867
##  [12,] 590.2330 -658.9788
##  [13,] 587.0030 -655.3685
##  [14,] 583.5505 -651.9704
##  [15,] 579.8894 -648.7980
##  [16,] 576.0344 -645.8642
##  [17,] 572.0012 -643.1809
##  [18,] 567.8059 -640.7587
##  [19,] 563.4654 -638.6075
##  [20,] 558.9972 -636.7359
##  [21,] 554.4193 -635.1515
##  [22,] 549.7502 -633.8606
##  [23,] 545.0086 -632.8685
##  [24,] 540.2135 -632.1791
##  [25,] 535.3845 -631.7951
##  [26,] 530.5407 -631.7183
##  [27,] 525.7019 -631.9488
##  [28,] 520.8874 -632.4857
##  [29,] 516.1167 -633.3269
##  [30,] 511.4089 -634.4690
##  [31,] 506.7831 -635.9074
##  [32,] 502.2578 -637.6363
##  [33,] 497.8512 -639.6487
##  [34,] 493.5812 -641.9365
##  [35,] 489.4648 -644.4905
##  [36,] 485.5188 -647.3005
##  [37,] 481.7589 -650.3551
##  [38,] 478.2003 -653.6420
##  [39,] 474.8573 -657.1480
##  [40,] 471.7434 -660.8590
##  [41,] 468.8712 -664.7600
##  [42,] 466.2522 -668.8353
##  [43,] 463.8969 -673.0685
##  [44,] 461.8148 -677.4426
##  [45,] 460.0144 -681.9399
##  [46,] 458.5028 -686.5423
##  [47,] 457.2861 -691.2314
##  [48,] 456.3693 -695.9882
##  [49,] 455.7561 -700.7935
##  [50,] 455.4488 -705.6281
##  [51,] 455.4488 -710.4724
##  [52,] 455.7561 -715.3070
##  [53,] 456.3693 -720.1123
##  [54,] 457.2861 -724.8691
##  [55,] 458.5028 -729.5582
##  [56,] 460.0144 -734.1606
##  [57,] 461.8148 -738.6579
##  [58,] 463.8969 -743.0320
##  [59,] 466.2522 -747.2652
##  [60,] 468.8712 -751.3405
##  [61,] 471.7434 -755.2415
##  [62,] 474.8573 -758.9524
##  [63,] 478.2003 -762.4584
##  [64,] 481.7589 -765.7454
##  [65,] 485.5188 -768.8000
##  [66,] 489.4648 -771.6099
##  [67,] 493.5812 -774.1640
##  [68,] 497.8512 -776.4518
##  [69,] 502.2578 -778.4642
##  [70,] 506.7831 -780.1931
##  [71,] 511.4089 -781.6315
##  [72,] 516.1167 -782.7736
##  [73,] 520.8874 -783.6148
##  [74,] 525.7019 -784.1517
##  [75,] 530.5407 -784.3822
##  [76,] 535.3845 -784.3054
##  [77,] 540.2135 -783.9214
##  [78,] 545.0086 -783.2320
##  [79,] 549.7502 -782.2399
##  [80,] 554.4193 -780.9490
##  [81,] 558.9972 -779.3646
##  [82,] 563.4654 -777.4930
##  [83,] 567.8059 -775.3418
##  [84,] 572.0012 -772.9196
##  [85,] 576.0344 -770.2363
##  [86,] 579.8894 -767.3025
##  [87,] 583.5505 -764.1301
##  [88,] 587.0030 -760.7320
##  [89,] 590.2330 -757.1217
##  [90,] 593.2276 -753.3138
##  [91,] 595.9746 -749.3236
##  [92,] 598.4630 -745.1673
##  [93,] 600.6828 -740.8615
##  [94,] 602.6250 -736.4235
##  [95,] 604.2819 -731.8714
##  [96,] 605.6467 -727.2233
##  [97,] 606.7140 -722.4980
##  [98,] 607.4794 -717.7145
##  [99,] 607.9399 -712.8921
## [100,] 608.0936 -708.0502
# Perform eigenvalue decomposition
eig <- eigen(sigma2)

# Calculate chi-square critical value for 95% confidence
chi_critical <- qchisq(0.95, df = 2)

# Calculate the ellipse axes lengths
ellipse_axes <- sqrt(chi_critical * eig$values)

# Calculate the rotation angle
rotation_angle <- atan2(eig$vectors[2, 1], eig$vectors[1, 1])

# Generate points for the ellipse
theta <- seq(0, 2 * pi, length.out = 100)
ellipse <- cbind(ellipse_axes[1] * cos(theta), ellipse_axes[2] * sin(theta))
rotation_matrix <- matrix(c(cos(rotation_angle), -sin(rotation_angle), sin(rotation_angle), cos(rotation_angle)), ncol = 2)
rotated_ellipse <- t(t(rotation_matrix %*% t(ellipse)))

# Plot the data with the confidence ellipse
ggplot() +
  geom_point(aes(x[[4]][1], x[[4]][4])) +
  stat_ellipse(type = "norm", level = 0.95, linetype = "dashed", color = "blue") +
  theme_minimal()

Vehicle X-axis velocity

# Take the velocity estimates in the x axis
estimatesX <- c()
for (i in seq(3, 70, by = 2)){
    estimatesX = append(estimatesX, list(x[[i]][2]))
}

# Dataframa with all the data
dataVx <- data.frame(
  recta = c(1:34),
  estimX = unlist(estimatesX)
)
# True value?
# Plot to compare the true value, measured values and estimates
plotVx <- ggplot(dataVx, aes(x = recta)) +
  geom_path(aes(y = estimX, color = "Estimates")) +
  geom_point(aes(y = estimX, color = "Estimates", shape = "Estimates")) +
  
  labs(title = "Vehicle X-axis velocity",
       x = "Time(s)",
       y = "Vx(m/s)",
       color = "") +
  
  scale_color_manual(values = c("Estimates" = "red", "True values" = "green"), name = "") +
  scale_shape_manual(values = c("Estimates" = 19, "True values" = 18), name = "")

ggplotly(plotVx) %>% config(displayModeBar = TRUE)

Vehicle Y-axis velocity

# Take the velocity estimates in the y axis
estimatesY <- c()
for (i in seq(3, 70, by = 2)){
    estimatesY = append(estimatesY, list(x[[i]][5]))
}

# Dataframa with all the data
dataVy <- data.frame(
  recta = c(1:34),
  estimY = unlist(estimatesY)
)
# True value?
# Plot to compare the true value, measured values and estimates
plotVy <- ggplot(dataVy, aes(x = recta)) +
  geom_path(aes(y = estimY, color = "Estimates")) +
  geom_point(aes(y = estimY, color = "Estimates", shape = "Estimates")) +
  
  labs(title = "Vehicle Y-axis velocity",
       x = "Time(s)",
       y = "Vy(m/s)",
       color = "") +
  
  scale_color_manual(values = c("Estimates" = "red", "True values" = "green"), name = "") +
  scale_shape_manual(values = c("Estimates" = 19, "True values" = 18), name = "")

ggplotly(plotVy) %>% config(displayModeBar = TRUE)